/*
 * VelocityPID.cpp
 *
 *  Created on: 28.12.2012
 *      Author: Michael
 */

#include "rodos.h"
#include "VelocityPID.h"
#include "../Threads/global.h"
#include "../hardware.h"

#include <stdlib.h>
#include "../Threads/global.h"

extern Systemstate state;
extern PowerDriver motor;

VelocityPID::VelocityPID() {
	pre_error_v = 0;
	integral_v = 0;
	output_v = 0;
	P = 0;
	I = 0;
	D = 0;
//	state.k_pVel = -3.;
//	state.k_iVel = -0.05;
//	state.k_dVel = -0.21;
	state.k_pVel = -1.;
	state.k_iVel = -0.005;
	state.k_dVel = -0.0035;
}

VelocityPID::~VelocityPID() {
	// TODO Auto-generated destructor stub
}

void VelocityPID::resetPID(){
	pre_error_v = 0;
	integral_v = 0;
	output_v = 0;
	P = 0;
	I = 0;
	D = 0;
}

void VelocityPID::run() {
	xprintf("I'm here velocity\n");

	float error = 0;
	float derivative = 0;

	error = state.desVel - (state.velGyro - state.gyroBias);
//	error = state.desVel - state.velGyro;

	// Avoid integration for too small errors
	if (abs(error) > state.eps_v) {
		integral_v += error * SAMPLETIME_S;
	}
	derivative = (error - pre_error_v) / SAMPLETIME_S;

	P = state.k_pVel * error;
	I = state.k_iVel * integral_v;
	D = state.k_dVel * derivative;
	output_v = P + I + D;

	// Save the error
	pre_error_v = error;

	if(output_v > LIMIT_VOLTAGE){
		output_v = LIMIT_VOLTAGE;
	} else if (output_v < -LIMIT_VOLTAGE){
		output_v = -LIMIT_VOLTAGE;
	}

	state.dutycycle = output_v / MAX_VOLTAGE * 100;

	motor.setSpeedPercentage(state.dutycycle);
}
